{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# GPSFactor Family\n",
    "\n",
    "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/navigation/doc/GPSFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>\n",
    "\n",
    "## Overview\n",
    "\n",
    "The `GPSFactor` family provides factors for incorporating Global Positioning System (GPS) measurements into a GTSAM factor graph. GPS typically provides measurements of position in Latitude/Longitude/Height. These GPS factors, however, assume the GPS measurement has been converted into a local Cartesian **navigation frame** (e.g., [ENU, NED, or ECEF](https://dirsig.cis.rit.edu/docs/new/coordinates.html)).\n",
    "\n",
    "Different variants exist to handle:\n",
    "- State type: `Pose3` or `NavState`.\n",
    "- Lever arm: Whether the GPS antenna is offset from the body frame origin.\n",
    "- Calibration: Whether the lever arm itself is being estimated."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "tags": [
     "remove-cell"
    ]
   },
   "outputs": [],
   "source": [
    "%pip install --quiet gtsam-develop"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Factor Variants\n",
    "\n",
    "**For `Pose3` states:**\n",
    "\n",
    "- **`GPSFactor`**: \n",
    "  - Connects to: `Pose3` key.\n",
    "  - Assumes: Zero lever arm (GPS measurement corresponds directly to the `Pose3` origin).\n",
    "  - Measurement: 3D position (`Point3`) in the navigation frame.\n",
    "  - Error: $p_{pose} - p_{measured}$\n",
    "\n",
    "- **`GPSFactorArm`**: \n",
    "  - Connects to: `Pose3` key.\n",
    "  - Assumes: Known, fixed lever arm (`bL` vector in the body frame).\n",
    "  - Measurement: 3D position (`Point3`) in the navigation frame.\n",
    "  - Error: $(p_{pose} + R_{nb} \\cdot bL) - p_{measured}$\n",
    "\n",
    "- **`GPSFactorArmCalib`**: \n",
    "  - Connects to: `Pose3` key, `Point3` key (for the lever arm).\n",
    "  - Assumes: Lever arm (`bL`) is unknown and estimated.\n",
    "  - Measurement: 3D position (`Point3`) in the navigation frame.\n",
    "  - Error: $(p_{pose} + R_{nb} \\cdot bL_{estimated}) - p_{measured}$\n",
    "\n",
    "**For `NavState` states:**\n",
    "\n",
    "- **`GPSFactor2`**: Like `GPSFactor` but connects to a `NavState` key.\n",
    "- **`GPSFactor2Arm`**: Like `GPSFactorArm` but connects to a `NavState` key.\n",
    "- **`GPSFactor2ArmCalib`**: Like `GPSFactorArmCalib` but connects to a `NavState` key and a `Point3` lever arm key.\n",
    "\n",
    "(The '2' suffix historically denoted factors using `NavState`)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Mathematical Formulation (GPSFactorArm Example)\n",
    "\n",
    "Let:\n",
    "- $T_{nb} = (R_{nb}, p_{nb})$ be the `Pose3` state (body frame $b$ in navigation frame $n$).\n",
    "- $L_b$ be the known lever arm vector from the body origin to the GPS antenna, expressed in the body frame.\n",
    "- $p_{gps}$ be the measured GPS position in the navigation frame.\n",
    "\n",
    "The predicted position of the GPS antenna in the navigation frame is:\n",
    "$$ p_{ant, pred} = p_{nb} + R_{nb} \\cdot L_b $$ \n",
    "\n",
    "The factor's 3D error vector is the difference between the predicted antenna position and the measured GPS position:\n",
    "$$ e = p_{ant, pred} - p_{gps} $$ \n",
    "\n",
    "The noise model reflects the uncertainty of the $p_{gps}$ measurement in the navigation frame."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Key Functionality / API (Common Patterns)\n",
    "\n",
    "- **Constructor**: Takes the relevant key(s), the measured `Point3` position `gpsIn` (in nav frame), the noise model, and potentially the `leverArm` (`Point3` in body frame).\n",
    "- **`evaluateError(...)`**: Calculates the 3D error vector based on the connected state variable(s) and the measurement.\n",
    "- **`measurementIn()`**: Returns the stored `Point3` measurement.\n",
    "- **`leverArm()`** (For Arm variants): Returns the stored `Point3` lever arm."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Usage Example (GPSFactor and GPSFactorArm)\n",
    "\n",
    "Assume we have a GPS reading converted to a local ENU frame."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Created GPSFactor (zero lever arm):\n",
      "GPSFactor on x0\n",
      "  GPS measurement: 10.5\n",
      "20.2\n",
      " 5.1\n",
      "  noise model: diagonal sigmas [0.5; 0.5; 1];\n",
      "\n",
      "GPSFactor Error: [-0.5 -0.2 -0.1]\n",
      "\n",
      "Created GPSFactorArm:\n",
      "GPSFactorArm on x0\n",
      "  GPS measurement: 10.5 20.2  5.1\n",
      "  Lever arm: -0.1    0 0.05\n",
      "  noise model: diagonal sigmas [0.5; 0.5; 1];\n",
      "\n",
      "GPSFactorArm Error: [-0.6  -0.2  -0.05]\n",
      " ( Pose:       [10. 20.  5.] )\n",
      " ( Lever Arm:  [-0.1   0.    0.05] )\n",
      " ( Predicted Antenna Pos:  [ 9.9  20.    5.05] )\n",
      " ( Measured GPS Pos:     [10.5 20.2  5.1] )\n",
      "\n",
      "Created GPSFactorArmCalib:\n",
      "GPSFactorArmCalib on x0\n",
      "  GPS measurement: 10.5 20.2  5.1\n",
      "  noise model: diagonal sigmas [0.5; 0.5; 1];\n"
     ]
    }
   ],
   "source": [
    "import gtsam\n",
    "import numpy as np\n",
    "from gtsam.symbol_shorthand import X, L # Pose key, Lever arm key\n",
    "\n",
    "# --- Setup ---\n",
    "pose_key = X(0)\n",
    "\n",
    "# GPS Measurement in local ENU frame (meters)\n",
    "gps_measurement_enu = gtsam.Point3(10.5, 20.2, 5.1)\n",
    "\n",
    "# Noise model for GPS measurement (e.g., 0.5m horizontal, 1.0m vertical sigma)\n",
    "gps_sigmas = np.array([0.5, 0.5, 1.0])\n",
    "gps_noise_model = gtsam.noiseModel.Diagonal.Sigmas(gps_sigmas)\n",
    "\n",
    "# --- Scenario 1: GPSFactor (Zero Lever Arm) ---\n",
    "gps_factor_zero_arm = gtsam.GPSFactor(pose_key, gps_measurement_enu, gps_noise_model)\n",
    "print(\"Created GPSFactor (zero lever arm):\")\n",
    "gps_factor_zero_arm.print()\n",
    "\n",
    "# Evaluate error: Error is difference between pose translation and measurement\n",
    "test_pose1 = gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(10.0, 20.0, 5.0))\n",
    "error1 = gps_factor_zero_arm.evaluateError(test_pose1)\n",
    "print(\"\\nGPSFactor Error:\", error1) # Expected: [0.5, 0.2, 0.1]\n",
    "\n",
    "# --- Scenario 2: GPSFactorArm (Known Lever Arm) ---\n",
    "# Assume antenna is 10cm behind and 5cm above the body origin\n",
    "lever_arm_body = gtsam.Point3(-0.1, 0.0, 0.05) \n",
    "\n",
    "gps_factor_with_arm = gtsam.GPSFactorArm(pose_key, gps_measurement_enu, \n",
    "                                         lever_arm_body, gps_noise_model)\n",
    "print(\"\\nCreated GPSFactorArm:\")\n",
    "gps_factor_with_arm.print()\n",
    "\n",
    "# Evaluate error: Error is difference between (pose + R*lever_arm) and measurement\n",
    "# Use the same test pose as before\n",
    "predicted_antenna_pos = test_pose1.transformFrom(lever_arm_body)\n",
    "error2 = gps_factor_with_arm.evaluateError(test_pose1)\n",
    "print(\"\\nGPSFactorArm Error:\", error2) \n",
    "print(\" ( Pose:      \", test_pose1.translation() , \")\")\n",
    "print(\" ( Lever Arm: \", lever_arm_body, \")\")\n",
    "print(\" ( Predicted Antenna Pos: \", predicted_antenna_pos, \")\")\n",
    "print(\" ( Measured GPS Pos:    \", gps_measurement_enu, \")\")\n",
    "# Expected: predicted_antenna_pos - gps_measurement_enu \n",
    "#           = [9.9, 20.0, 5.05] - [10.5, 20.2, 5.1] = [-0.6, -0.2, -0.05]\n",
    "\n",
    "# --- Scenario 3: GPSFactorArmCalib (Example Setup - Not Evaluated) ---\n",
    "lever_arm_key = L(0) # Key for the unknown lever arm\n",
    "gps_factor_calib = gtsam.GPSFactorArmCalib(pose_key, lever_arm_key, \n",
    "                                            gps_measurement_enu, gps_noise_model)\n",
    "print(\"\\nCreated GPSFactorArmCalib:\")\n",
    "gps_factor_calib.print()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Coordinate Frames\n",
    "\n",
    "It's crucial to ensure consistency in coordinate frames:\n",
    "- **GPS Measurement (`gpsIn`)**: Must be provided in the chosen local Cartesian **navigation frame** (e.g., ENU, NED).\n",
    "- **Lever Arm (`leverArm`)**: Must be provided in the **body frame**.\n",
    "- **Pose/NavState Variables**: Represent the pose of the body frame in the navigation frame ($T_{nb}$)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Source\n",
    "- [GPSFactor.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/GPSFactor.h)\n",
    "- [GPSFactor.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/GPSFactor.cpp)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "py312",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.12.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
